/* 简易滤波器，不要骂了，高数没学好QAQ*/
#include "filter.h"

#include <string.h>

int8_t MedianAverageFilter_Init(MedianAverageFilter_t* filter, uint8_t n) {
  if (filter == NULL) return -1;
  filter->ready = 0;
  memset(filter->data, 0, sizeof(filter->data));
  filter->counter = 0;
  filter->n = n;
  return 0;
}

float MedianAverageFilter_Apply(MedianAverageFilter_t* filter, float new) {
  if (filter == NULL) return -1;
  filter->data[filter->counter++] = new;
  if (filter->ready) {
    if (filter->counter >= filter->n) filter->counter = 0;
  } else if (filter->counter >= filter->n) {
    filter->counter = 0;
    filter->ready = 1;
  } else
    return 0;
  int tmp = 0;
  float sum = 0;
  filter->max = filter->data[0];
  filter->min = filter->data[0];
  while (++tmp < filter->n) {
    if (filter->data[tmp] < filter->min) filter->min = filter->data[tmp];
    if (filter->data[tmp] > filter->max) filter->max = filter->data[tmp];
  }
  for (tmp = 0; tmp < filter->n; tmp++) sum += filter->data[tmp];
  return (sum - filter->max - filter->min) / (float)(filter->n - 2.0f);
}

int8_t AverageFilter_Init(AverageFilter_t* filter, uint8_t n) {
  if (filter == NULL) return -1;
  filter->ready = 0;
  memset(filter->data, 0, sizeof(filter->data));
  filter->counter = 0;
  filter->n = n;
  return 0;
}

float AverageFilter_Apply(AverageFilter_t* filter, float new) {
  if (filter == NULL) return -1;
  filter->data[filter->counter++] = new;
  if (filter->ready) {
    if (filter->counter >= filter->n) filter->counter = 0;
  } else if (filter->counter >= filter->n) {
    filter->counter = 0;
    filter->ready = 1;
  } else
    return 0;
  int tmp = 0;
  float sum = 0;
  for (tmp = 0; tmp < filter->n; tmp++) sum += filter->data[tmp];
  return sum / (float)(filter->n);
}

int8_t LowPassFilter_Init(LowPassFilter_t* filter, float k) {
  if (filter == NULL) return -1;
  filter->k = k;
  return 0;
}

float LowPassFilter_Apply(LowPassFilter_t* filter, float new) {
  if (filter == NULL) return -1;
  filter->out = filter->k * new + (1 - filter->k) * filter->out;
  return filter->out;
}

int8_t ComplementaryFilter_Init(ComplementaryFilter_t* filter, float k,
                                uint32_t freq) {
  if (filter == NULL) return -1;
  filter->k = k;
  filter->freq = freq;
  return 0;
}

float ComplementaryFilter_Apply(ComplementaryFilter_t* filter, float angle,
                                float gyro) {
  if (filter == NULL) return -1;
  filter->out =
      filter->k * angle + (1 - filter->k) * (gyro / filter->freq + filter->out);
  return filter->out;
}
